/**
 * Copyright (C) 2022, RAM-LAB, Hong Kong University of Science and Technology
 * This file is part of GPIR (https://github.com/jchengai/gpir).
 * If you find this repo helpful, please cite the respective publication as
 * listed on the above website.
 */

#pragma once

#include <ackermann_msgs/AckermannDrive.h>

#include <Eigen/Dense>
#include <deque>
#include <vector>
#include <iostream>
#include <cmath>
#include <tf/tf.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>

#include "common/state.h"
#include "common/trajectory.h"
#include "common/boxcar_filter.h"

using namespace std;

class MpcController
{
public:
    MpcController();

    void Init();

    void set_wheel_base(const double wheel_base) { ls_ = wheel_base; }

    bool CalculateAckermannDrive(const common::State &state,
                                 const common::Trajectory &trajectory,
                                 std::vector<double> *control_cmd);

    void mpc_control(const nav_msgs::Odometry &robot_state, const nav_msgs::Path &trj_point,
                     const vector<double> trj_k, const vector<double> trj_t, const vector<double> trj_v, double &out_turn_agl, int &out_index);

protected:
    void Reset(const common::State &state);

    void Discretization(const Eigen::MatrixXd &matrix_a,
                        const Eigen::MatrixXd &matrix_b);

    void RecordControlCommand(const double speed, const double steer);

private:
    const int nx_ = 3;
    const int nu_ = 2;

    double ts_ = 0.05;
    double ls_ = 0.0;
    int horizon_ = 20;
    int predict_steps_ = 2;

    double max_speed_ = 20.0;
    double min_speed_ = 0.0;
    double max_acc_ = 3.0;
    double min_acc_ = -5.0;
    double max_steer_ = 60.0 * M_PI / 180.0;
    double max_srate_ = 40.0 * M_PI / 180.0;

    Eigen::MatrixXd matrix_a_piao_;
    Eigen::MatrixXd matrix_b_piao_;
    Eigen::MatrixXd matrix_ad_;
    Eigen::MatrixXd matrix_bd_;
    Eigen::MatrixXd matrix_q_;
    Eigen::MatrixXd matrix_r_;
    Eigen::MatrixXd matrix_xmax_;
    Eigen::MatrixXd matrix_xmin_;
    Eigen::MatrixXd matrix_xmax_relative_;
    Eigen::MatrixXd matrix_xmin_relative_;
    Eigen::MatrixXd matrix_umax_;
    Eigen::MatrixXd matrix_umin_;

    common::BoxCarFilter bf_x_;
    common::BoxCarFilter bf_y_;
    common::BoxCarFilter bf_heading_;
    common::BoxCarFilter bf_steer_;

    std::vector<double> last_control_;

    std::deque<double> history_vel_;
    std::deque<double> history_steer_;
};
